#include "Accelerometre.h"
#include "Chronometer.h"
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <sstream>
#include <cstdio>
#include <fcntl.h>
#include <iomanip>

CDatapoolServer::Cdata_t cd_AccelerationX;
CDatapoolServer::Cdata_t cd_AccelerationY;

double m_AccelerationX;
double m_AccelerationY;

Accelerometre::Accelerometre():
m_GpioXFd(-1),
m_GpioYFd(-1),
m_shallStop(true),
m_ProductionThread(NULL),
m_XAccumulator(NULL),
m_YAccumulator(NULL),
cd_AccelerationXID(-1),
cd_AccelerationYID(-1)
{
	cd_AccelerationX.type = CDatapoolServer::Double;
	cd_AccelerationY.type = CDatapoolServer::Double;
	cd_AccelerationX.value = &m_AccelerationX;
	cd_AccelerationY.value = &m_AccelerationY;
	cd_AccelerationXID = DataServer.RegisterData(&cd_AccelerationX);
	cd_AccelerationYID = DataServer.RegisterData(&cd_AccelerationY);

	m_XAccumulator = new boost::circular_buffer<int>(10);
	m_YAccumulator = new boost::circular_buffer<int>(10);
}

int Accelerometre::ReadX()
{
    if(m_GpioXFd == -1)
	{

		std::stringstream MuxFileName;
		MuxFileName<< "/sys/kernel/debug/omap_mux/ain1";
		int RxFd = open(MuxFileName.str().c_str(), O_WRONLY);
		write(RxFd , "0x8000", 6);
		close(RxFd);

		std::stringstream FileName;
		FileName << "/sys/devices/platform/omap/tsc/ain1";

		m_GpioXFd = open(FileName.str().c_str(),O_RDONLY);
	}

	char buffer[1024];
	read(m_GpioXFd, &buffer, sizeof(buffer));
	lseek(m_GpioXFd, 0, 0);
	return atoi(buffer);
}

int Accelerometre::ReadY()
{
	if(m_GpioYFd == -1)
	{

		std::stringstream MuxFileName;
		MuxFileName<< "/sys/kernel/debug/omap_mux/ain2";
		int RxFd = open(MuxFileName.str().c_str(), O_WRONLY);
		write(RxFd , "0x8000", 6);
		close(RxFd);

		std::stringstream FileName;
		FileName << "/sys/devices/platform/omap/tsc/ain2";

		m_GpioYFd = open(FileName.str().c_str(),O_RDONLY);
	}

	char buffer[1024];
	read(m_GpioYFd, &buffer, sizeof(buffer));
	lseek(m_GpioYFd, 0, 0);
	return atoi(buffer);
}

Accelerometre::~Accelerometre()
{
	close(m_GpioXFd);
	close(m_GpioYFd);
	this->Stop();
	delete m_XAccumulator;
	delete m_YAccumulator;
}

void Accelerometre::ProductionThread()
{
	double Factor = 7./4096;

	bool firstTime = true;

	double NoMoveX = 0, NoMoveY = 0;
	double LastVitesseX = 0, LastVitesseY = 0;
	double LastPositionX = 0, LastPositionY = 0;

	double InputX = 0, InputY = 0;
	double OutputX = 0, OutputY = 0;
	double lastInputX = 0, lastInputY = 0;
	double lastOutputX = 0, lastOutputY = 0;

	//low pass filter params
	double Samplef = 1. / 0.01; //sampling frequency
	double Corner = 5.; //cut off frequency
	double alpha = tan( 3.14159265359 * Corner/Samplef);
	double a = (1. - alpha)/(1. + alpha);
	double b = (1. - a) / 2.;

	int LoopIndex = 0;

	Chronometer timer;
	int sleepTime = 0;

	while(false == m_shallStop)
	{
		timer.Start();

		m_XAccumulator->push_back(ReadX());
		m_YAccumulator->push_back(ReadY());

		if(LoopIndex == 9)
		{
			boost::circular_buffer<int>::iterator XIt = m_XAccumulator->begin();
			boost::circular_buffer<int>::iterator YIt = m_YAccumulator->begin();

			int XCumul = 0;
			int YCumul = 0;

			for(int i = 0; i < 10; i++)
			{
				XCumul += *XIt;
				YCumul += *YIt;
			}

			if(firstTime)
			{
				NoMoveX = (static_cast<double>(XCumul/10) * Factor);
				NoMoveY = (static_cast<double>(YCumul/10) * Factor);
				if(LoopIndex == 9)
					firstTime = false;
			}
			else
			{
				InputX = ((static_cast<double>(XCumul/10) * Factor) - NoMoveX) * 9.81;
				std::cout << std::setw(8) << std::setprecision(2) << InputX << " ";

				InputY = ((static_cast<double>(YCumul/10) * Factor) - NoMoveY) * 9.81;
				std::cout << std::setw(8) << std::setprecision(2) << InputY << " ";

				//low pass filter
				OutputX = b * InputX + b * lastInputX + a * OutputX;
				OutputY = b * InputY + b * lastInputY + a * OutputY;

				lastInputX = InputX;
				lastInputY = InputY;

				m_AccelerationX = OutputX;
				m_AccelerationY = OutputY;

				std::cout << std::setw(8) << std::setprecision(2) << m_AccelerationX << " ";
				std::cout << std::setw(8) << std::setprecision(2) << m_AccelerationY << " ";

				double VelocityX = m_AccelerationX * 0.1 + LastVitesseX;
				std::cout << std::setw(8) << std::setprecision(2) << VelocityX << " ";
				LastVitesseX = VelocityX;

				double VelocityY = m_AccelerationY * 0.1 + LastVitesseY;
				std::cout << std::setw(8) << std::setprecision(2) << VelocityY << " ";
				LastVitesseY = VelocityY;

				double PositionX = VelocityX * 0.1 + LastPositionX;
				std::cout << std::setw(8) << std::setprecision(2) << PositionX << " ";
				LastPositionX = PositionX;

				double PositionY = VelocityY * 0.1 + LastPositionY;
				std::cout << std::setw(8) << std::setprecision(2) << PositionY << std::endl;
				LastPositionY = PositionY;
			}
		}

		LoopIndex++;
		if(LoopIndex == 10)
		{
			LoopIndex = 0;
		}

		timer.Stop();
		sleepTime = 10 - timer.Duration();
		boost::this_thread::sleep(boost::posix_time::milliseconds(sleepTime));
	}
}

void Accelerometre::Run()
{
	m_shallStop = false;
	boost::function<void (void)>Production = boost::bind(&Accelerometre::ProductionThread, this);
	m_ProductionThread = new boost::thread(Production);
}

void Accelerometre::Stop()
{
	m_shallStop = true;
	if(m_ProductionThread != NULL)
	{
		m_ProductionThread->join();
		delete m_ProductionThread;
	}
	std::cout << "ACCEL Stopped!" << std::endl;
}

